perm filename CAMERA.SAI[SYS,HE]12 blob
sn#067948 filedate 1973-10-24 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00022 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEGIN "GILCAM"
C00007 00003 ⊃ pot limits and misc. procedures
C00009 00004 ⊃ PRINT LIMITS
C00012 00005 ⊃ READS DATA IN GILL'S FORMAT
C00014 00006 ⊃ SERVO THE SELECTED CAMERA
C00017 00007 ⊃ Compute the transformation matrices for the camera model
C00019 00008 ⊃ CONTINUE GENERATING TRANSFORM
C00022 00009 ⊃ UPDATE THE TRANSFORM FOR SELECTED CAMERA
C00025 00010 ⊃ UPDATE CONTINUES
C00028 00011 ⊃ STILL MORE UPDATING
C00031 00012 ⊃ FINISH UPDATE
C00034 00013 ⊃ ERROR CHECKING ROUTINES
C00037 00014 ⊃ CHANGE LENS OR MOVE PAN/TILT HEAD
C00040 00015 ⊃ CHANGE LENS FOCUS AND ZOOM
C00042 00016 ⊃ CHANGE IRIS
C00043 00017 ⊃ CALCULATIONS COMMON TO PREDICTION AND CENTERING
C00045 00018 ⊃ CENTER CAMERA
C00050 00019 ⊃ TRANSFORM PREDICTION AND PROGRAM INITIALIZATION
C00055 00020 ⊃ PROGRAM TEST ROUTINE
C00057 00021 ⊃ CONTINUE TESTING
C00059 00022 ⊃ THIS IS THE MAIN PROGRAM
C00061 ENDMK
C⊗;
BEGIN "GILCAM"
REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
REQUIRE "HELIB[1,3]" LIBRARY;
REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
REQUIRE -1 NEW_ITEMS;
DEFINE ⊃="COMMENT", SAFEX="SAFE", XDATA="3", CRLF="&'15&'12",
YES="INCHWL=""Y""",KY="684.5", C1="2.1249", C2="-.0006",
SAVE="CAMERR[CAMNUM]←CAMFLG",DEG="57.29578",
ERROR(I)="CAMFLG←CAMFLG LOR (I)",
CAMERA(I)=" IF ¬(1≤(CAMNUM←I)≤2) THEN
BEGIN ERROR('10000); SAVE; RETURN; END;
CAMJOB[CAMNUM] ← JOB;
IF I=2 THEN CAMLENS←5";
⊃ ERRORS: 1-SERVO PROBLEMS(SERVO).
2-NOT ENOUGH READINGS(CAM_UPDATE).
4-POTS TOO NOISY(CAM_UPDATE).
10-LENS OUT OF BOUNDS(CHNG_LENS,CAM_CENTER).
20-PAN OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
40-TILT OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
100-FOCUS OUT OF BOUNDS(CHNG_FOCUS,CAM_CENTER).
200-ZOOM OUT OF BOUNDS
400-IRIS OUT OF BOUNDS
1000-LENS CHANGER GOOFED(CHNG_LENS).
2000-LOOKUP FAILED(DATXFR).
10000-ILLEGAL CAMERA NUMBER
20000-AD NOT AVAILABLE ;
EXTERNAL PROCEDURE CALLEN;
EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
EXTERNAL PROCEDURE SPWOFF;
EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);
FORTRAN REAL PROCEDURE ACOS(REAL X);
FORTRAN REAL PROCEDURE SQRT(REAL X);
FORTRAN REAL PROCEDURE COS(REAL X);
FORTRAN REAL PROCEDURE SIN(REAL X);
SHORT REAL TILPOT,FOCPOT,ZOOPOT,PANPOT,CAMPAN,CAMTIL,CAMRANG,CAMZOOM,IPOT,
ZPOT0,ZPOTD,REF,FOC_LGTH,RANG,PAN,TILT;
SHORT INTEGER MESS,BLOCK,CAMNUM,CAMLENS, CAMFLG, JOB;
EXTERNAL SHORT REAL L1,L2,L3,L4,L5,L6,L7,L8,e1,e2,e3,P1,P2,P3,P4,P5,P6,P7,P8,
SREF,CREF;
EXTERNAL SHORT INTEGER LENS, STATUS, TSERVO, STATE, SSERVO, POT;
SAFEX REAL ARRAY MCOL,MICOL [1:3,1:3]; ⊃ WHERE I KEEP CURRENT CAMERA XFORM;
SAFEX REAL ARRAY LCEN[1:3]; ⊃ AND LENS CENTER ;
SAFEX REAL ARRAY MODEL, PREDIC[1:10,1:3]; ⊃ THESE ARE THE MODELS;
SAFEX REAL ARRAY PPOT0,PPOTD,TPOT0,TPOTD,FPOT0,FPOTD,MART,SWING,GROREF,
FOC,FOCLEN0,FOCLENG[1:5],DP,P0[1:5,1:3],PP[1:5,1:2];
⊃ pot limits and misc. procedures;
preload_with 2020.,1850.;
SAFEX real array plimit1[1:2];
preload_with -2020.,154.;
SAFEX real array plimit2[1:2];
preload_with 2030.,2064.;
SAFEX real array tlimit1[1:2];
preload_with -1850.,-850.;
SAFEX real array tlimit2[1:2];
preload_with 2000.,4096.;
SAFEX real array flimit1[1:2];
preload_with -1720.0,-4096.;
SAFEX real array flimit2[1:2];
preload_with -2048.,-790.;
safex real array ilim1[1:2];
preload_with 2048.,1630.;
safex real array ilim2[1:2];
define zlim1="-1715.0", zlim2="1742.0";
⊃ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
BEGIN REAL K,M,N,Y,Z;
K←A*C;
M←A↑2+B↑2;
N←B↑2-C↑2;
Y ← K/M;
Z ← SQRT(Y↑2+(N/M));
RETURN(ACOS(-Y+(IF B<0 THEN Z ELSE -Z)));
END "COSQR";
SIMPLE REAL PROCEDURE LNS(REAL X; INTEGER RLENS);
RETURN (X*FOC[RLENS]/(X-FOC[RLENS]));
⊃ PRINT LIMITS;
SIMPLE PROCEDURE LIMPRT;
BEGIN INTEGER I, J, K;
SETFORMAT(1,0);
OUTSTR(NULL CRLF&"CAMERA "&CVS(CAMNUM)&" :"CRLF);
J ← IF CAMNUM=1 THEN 1 ELSE 5;
I ← IF CAMNUM=1 THEN 4 ELSE 5;
FOR CAMLENS ← J STEP 1 UNTIL I DO
BEGIN "LENS"
SETFORMAT(1,0);
OUTSTR(" LENS "&CVS(CAMLENS)&" :"CRLF);
SETFORMAT(7,2);
OUTSTR(" FOCUS RANGE IS ");
OUTSTR(CVF(LNS(FOCLEN0[CAMLENS]+FOCLENG[CAMLENS]*
FLIMIT2[CAMNUM],CAMLENS)));
OUTSTR(" TO "&CVF(LNS(FOCLEN0[CAMLENS]+
FOCLENG[CAMLENS]*FLIMIT1[CAMNUM],CAMLENS))&
" INCHES"CRLF);
OUTSTR(" PAN RANGE IS ");
OUTSTR(CVF((PPOTD[CAMLENS]*PLIMIT2[CAMNUM]+
PPOT0[CAMLENS])*DEG));
OUTSTR(" TO "&CVF((PPOTD[CAMLENS]*PLIMIT1[CAMNUM]+
PPOT0[CAMLENS])*DEG)&" DEGREES"CRLF);
OUTSTR(" TILT RANGE IS ");
OUTSTR(CVF((TPOTD[CAMLENS]*TLIMIT2[CAMNUM]+
TPOT0[CAMLENS])*DEG));
OUTSTR(" TO "&CVF((TPOTD[CAMLENS]*TLIMIT1[CAMNUM]+
TPOT0[CAMLENS])*DEG)&" DEGREES"CRLF);
IF CAMNUM=2 THEN
BEGIN "ZOOM"
OUTSTR(" ZOOM RANGE IS ");
OUTSTR(CVF((C1*ZLIM1+C2)*25.4));
OUTSTR(" TO "&CVF((C1*ZLIM2+C2)*25.4)&
" MM" CRLF);
END "ZOOM";
END "LENS";
END "LIMPRT";
⊃ READS DATA IN GILL'S FORMAT;
SIMPLE PROCEDURE DATXFR;
BEGIN
BOOLEAN FLAG;
INTEGER DUMMY;
REQUIRE "⊂⊃⊂⊃" DELIMITERS;
DEFINE DATASET= ⊂(IF CAMNUM=1 THEN "DATA[SYS,HE]"
ELSE "DATA2[SYS,HE]")⊃;
REQUIRE UNSTACK_DELIMITERS;
DEFINE XFR(X)="ARRYIN(XDATA,X,1)";
OPEN(XDATA,"DSK",12,3,0,0,0,0);
LOOKUP(XDATA,DATASET,FLAG);
IF FLAG THEN
BEGIN
OUTSTR("DATXFR-FAILED: LOOKUP FAILED FOR "&DATASET CRLF);
ERROR('2000);
RETURN;
END;
IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
USETI(XDATA,1);
DUMMY←WORDIN(XDATA);
USETI(XDATA,BLOCK);
IF CAMNUM=2 THEN
BEGIN BLOCK←5; XFR(ZPOT0); XFR(ZPOTD); END;
XFR(PPOT0[BLOCK]);
XFR(PPOTD[BLOCK]);
XFR(TPOT0[BLOCK]);
XFR(TPOTD[BLOCK]);
XFR(FPOT0[BLOCK]);
XFR(FPOTD[BLOCK]);
XFR(MART[BLOCK]);
XFR(SWING[BLOCK]);
XFR("PP[BLOCK,1]");
XFR("PP[BLOCK,2]");
XFR("P0[BLOCK,1]");
XFR("P0[BLOCK,2]");
XFR("P0[BLOCK,3]");
XFR("DP[BLOCK,1]");
XFR("DP[BLOCK,2]");
XFR("DP[BLOCK,3]");
XFR(FOC[BLOCK]);
XFR(FOCLEN0[BLOCK]);
XFR(FOCLENG[BLOCK]);
XFR(GROREF[BLOCK]);
RELEASE(XDATA);
END "DATXFR";
⊃ SERVO THE SELECTED CAMERA;
SIMPLE PROCEDURE SERVO;
BEGIN INTEGER I, EOF;
LABEL LOPEN;
IF CAMNUM=1 THEN
BEGIN "CAM1SR"
SPWON(1,TSERVO);
WHILE ¬(STATUS LAND 1) DO;
IF (STATUS≥'100)∧(STATUS<'100000) THEN
BEGIN "SR1FAL"
OUTSTR("COHU SERVO-FAILED: STATUS="&
CVOS(STATUS)CRLF);
ERROR(1);
END "SR1FAL";
SPWOFF;
CALL(1,"SLEEP");
IF DEB_CAM THEN OUTSTR
("SERVO: E1="&CVF(E1)&" E2="&CVF(E2)&
" E3="&CVF(E3)CRLF&" L1="&CVF(L1)&
" L2="&CVF(L2)&" L3="&CVF(L3)CRLF&
" P1="&CVF(P1)&" P2="&CVF(P2)&
" P3="&CVF(P3)CRLF);
END "CAM1SR";
IF CAMNUM=2 THEN
BEGIN "CAM2SR"
I ← 20;
LOPEN: OPEN(1,"AD",'417,0,0,0,0,EOF←TRUE);
IF EOF THEN IF (I←I-1) THEN
BEGIN "NOAD"
CALL(1,"SLEEP");
GO TO LOPEN;
END "NOAD" ELSE STATE←'40 ELSE BEGIN "ADOK"
STATE ← POT ← 0;
SPWON(0,SSERVO);
WHILE ¬STATE DO;
SPWOFF;
END "ADOK";
IF (STATE≥'100)∧(STATE<'100000) THEN
BEGIN "SR2FAL"
OUTSTR("SIERRA SERVO FAILED: STATUS="&
CVOS(STATE)CRLF);
ERROR(1);
END "SR2FAL";
IF DEB_CAM THEN OUTSTR
("SERVO: L4="&CVF(L4)&" L5="&CVF(L5)&
" L6="&CVF(L6)&" L7="&CVF(L7)&
" L8="&CVF(L8)CRLF&" P4="&CVF(P4)&
" P5="& CVF(P5)&" P6="&CVF(P6)&
" P7="&CVF(P7)&" P8="&CVF(P8)CRLF);
END "CAM2SR";
END "SERVO";
⊃ Compute the transformation matrices for the camera model
given by LENS and the last pots readings;
PROCEDURE PANTIL_CAM(INTEGER L;REAL PPOT,TPOT,FPOT,ZPOT;
SAFEX REAL ARRAY COL,ICOL,CENTER);
BEGIN INTEGER I,J;
REAL ACC,FMX,FMY,PAN,TILT, A, B, CC, D, A1, A2, A3, A4, A5;
SAFEX REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
PAN ← PPOTD[L]*PPOT+PPOT0[L];
TILT ← TPOTD[L]*TPOT+TPOT0[L];
FMY ← FPOTD[L]*FPOT+FPOT0[L];
IF CAMNUM=2 THEN FMY ← FMY+ZPOTD/(ZPOT-ZPOT0);
FMX ← FMY*MART[L];
RP[2,3] ← -1;
RPT[1,1] ← RP[1,1] ← RP[3,2] ← B ← -SIN(PAN);
RP[3,1] ← A ← -(RPT[1,2] ← RP[1,2] ← COS(PAN));
RT[1,1] ← RS[3,3] ← 1;
RPT[2,3] ← D ← -(RT[2,2] ← RT[3,3] ← COS(TILT));
R[3,3] ← RPT[3,3] ← RT[2,3] ← CC ← -(RT[3,2] ← SIN(TILT));
RPT[2,1] ← A3 ← CC*A;
RPT[2,2] ← A4 ← CC*B;
R[3,1] ← RPT[3,1] ← -D*A;
R[3,2] ← RPT[3,2] ← -D*B;
RS[1,1] ← RS[2,2] ← A1 ← COS(A2 ← SWING[L]);
RS[2,1] ← A5 ← -(RS[1,2] ← SIN(A2));
R[1,1] ← A1*B-A5*A3;
R[1,2] ← -A1*A-A5*A4;
R[1,3] ← -A5*D;
R[2,1] ← A5*B+A1*A3;
R[2,2] ← -A5*A+A1*A4;
R[2,3] ← A1*D;
A ← DP[L,1];
B ← DP[L,2];
CC ← DP[L,3];
FOR I←1 STEP 1 UNTIL 3 DO
C[I]←P0[L,I]+RPT[1,I]*A+RPT[2,I]*B+RPT[3,I]*CC;
⊃ CONTINUE GENERATING TRANSFORM;
⊃ -FMX/333 and PP[2]-PP[1]/333 are corrections for
the non-orthogonality of the TV scan axes;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
COL[I,1]←R[I,1];
COL[I,2]←R[I,2];
ACC←0;
FOR J←1 STEP 1 UNTIL 3 DO ACC←ACC-R[I,J]*C[J];
COL[I,3]←ACC;
END;
A ← FMX/333.0;
B ← PP[L,2]-(CC←PP[L,1])/333.0;
FOR J←1 STEP 1 UNTIL 3 DO
BEGIN
COL[2,J]←-A*COL[1,J]+FMY*COL[2,J]+B*COL[3,J];
COL[1,J]←FMX*COL[1,J]+CC*COL[3,J];
END;
INVRT(COL,ICOL);
ARRTRAN(CENTER,C);
END "PANTIL_CAM";
⊃ UPDATE THE TRANSFORM FOR SELECTED CAMERA;
SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO);
BEGIN LABEL ETA;
REAL SFOC,STIL,SPAN,IND,FMAX,FMIN,TMAX,TMIN,PMAX,PMIN,SZOM,ZMAX,
ZMIN,DIFFOC,DIFTIL,DIFPAN,DIFZOM,SIND,IMAX,IMIN,DIFIRIS,SIRIS;
INTEGER I;
CAMERA(CAMNO);
ETA: SFOC←SPAN←STIL←SZOM←SIRIS←0;
FMAX←TMAX←PMAX←ZMAX←IMAX←-10000;
FMIN←TMIN←PMIN←ZMIN←IMIN←10000;
IF CAMNO=1 THEN
BEGIN "CM1UPD"
IMAX ← IMIN ← 0;
STATUS←1;
SPWON(1,TSERVO);
I ← '44;
FOR IND←0 STEP 1 UNTIL 39 DO
BEGIN "CM1LOP"
STATUS←I;
I ← 4;
WHILE ¬(STATUS LAND 1) DO;
IF '100≤STATUS<'100000 THEN DONE;
SFOC←SFOC+P1;
STIL←STIL+P2;
SPAN←SPAN+P3;
IF P1>FMAX THEN FMAX←P1;
IF P1<FMIN THEN FMIN←P1;
IF P2>TMAX THEN TMAX←P2;
IF P2<TMIN THEN TMIN←P2;
IF P3>PMAX THEN PMAX←P3;
IF P3<PMIN THEN PMIN←P3;
END "CM1LOP";
SPWOFF;
CAMLENS ← LENS+1;
END "CM1UPD";
IF CAMNO=2 THEN
BEGIN "CM2UPD"
FOR IND←0 STEP 1 UNTIL 39 DO
BEGIN "CM2LOP"
STATE ← 0;
POT ← 1;
SPWON(0,SSERVO);
WHILE ¬STATE DO;
SPWOFF;
IF ¬(STATE=1) THEN DONE;
SPAN←SPAN+P4;
SFOC←SFOC+P6;
STIL←STIL+P5;
SZOM←SZOM+P7;
SIRIS←SIRIS+P8;
⊃ UPDATE CONTINUES;
IF P4>PMAX THEN PMAX←P4;
IF P4<PMIN THEN PMIN←P4;
IF P5>TMAX THEN TMAX←P5;
IF P5<TMIN THEN TMIN←P5;
IF P6>FMAX THEN FMAX←P6;
IF P6<FMIN THEN FMIN←P6;
IF P7>ZMAX THEN ZMAX←P7;
IF P7<ZMIN THEN ZMIN←P7;
IF P8>IMAX THEN IMAX←P8;
IF P8<IMIN THEN IMIN←P8;
END "CM2LOP";
END "CM2UPD";
IF IND>0 THEN
BEGIN "READOK"
ref← if camno=1 then cref else GROREF[5];
FOCPOT←SFOC*REF/IND;
TILPOT←STIL*REF/IND;
PANPOT←SPAN*REF/IND;
IPOT ← SIRIS*REF/IND;
IF CAMNO=2 THEN L7←ZOOPOT←SZOM*REF/IND;
IF DEB_CAM THEN OUTSTR("CAM_UPDATE:"&
" FOCUS POT="&CVF(FOCPOT)&
" TILT POT="&CVF(TILPOT) CRLF&
" PAN POT="&CVF(PANPOT)&
" IRIS POT="&CVF(IPOT) CRLF&
(IF CAMNO=2 THEN " ZOOM POT="&CVF(ZOOPOT) CRLF
ELSE NULL));
IF IND<30 THEN
BEGIN "RDLOW"
OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS "&
CVS(IND)&" "CRLF);
ERROR(2);
END "RDLOW" ELSE BEGIN "RDHIGH"
REAL DP, DT, DF, DZ, DI;
DIFFOC←(FMAX-FMIN)*REF;
DIFTIL←(TMAX-TMIN)*REF;
DIFPAN←(PMAX-PMIN)*REF;
DIFIRIS←(IMAX-IMIN)*REF;
IF CAMNO=2 THEN DIFZOM←(ZMAX-ZMIN)*REF;
IF DEB_CAM THEN OUTSTR("CAM_UPDATE:"&
" FOCUS DIF.="&CVS(DIFFOC)&
" TILT DIF.="&CVS(DIFTIL) CRLF&
" PAN DIF.="&CVS(DIFPAN)&
" IRIS DIF.="&CVS(DIFIRIS) CRLF&
(IF CAMNO=2 THEN " ZOOM DIF.="&
CVS(DIFZOM) CRLF ELSE NULL));
SIND←4*SQRT(IND);
⊃ STILL MORE UPDATING;
DZ ← IF CAMNO=2 THEN DIFZOM/SIND ELSE 0;
DF←DIFFOC/SIND;
DT←DIFTIL/SIND;
DP←DIFPAN/SIND;
DI←DIFIRIS/SIND;
IF DI>.75∨DP>.75∨DT>.75∨DF>1∨(CAMNO=2∧DZ>1) THEN
BEGIN "DIFBAD"
OUTSTR("CAM_UPDATE: POTS TOO NOISY ");
IF DF>1.0 THEN OUTSTR(" DIFFOC="&
CVF(DIFFOC));
IF DT>.75 THEN OUTSTR(" DIFTIL="&
CVF(DIFTIL));
IF DP>.75 THEN OUTSTR(" DIFPAN="&
CVF(DIFPAN));
IF DI>.75 THEN OUTSTR(" DIFIRIS="&
CVF(DIFIRIS));
IF DZ>1.0 THEN OUTSTR(" DIFZOM="&
CVF(DIFZOM));
OUTSTR(NULL CRLF);
ERROR(4);
END "DIFBAD";
END "RDHIGH";
END "READOK" ELSE BEGIN "READBD"
OUTSTR("CAM_UPDATE: AD NOT AVAILABLE"CRLF);
ERROR('20000);
END "READBD";
IF (CAMFLG LAND '20007)≠0 THEN
BEGIN "ERROR"
OUTSTR("...TYPE Y TO TRY AGAIN:"CRLF);
IF YES THEN
BEGIN "RETRY"
CAMFLG←CAMFLG LAND '777777757770;
GOTO ETA
END "RETRY" ELSE
IF CAMFLG LAND '20000 THEN
BEGIN "ADERR"
OUTSTR("CAM_UPDATE-FAILED: CAMERA_MODEL NOT"&
" UPDATED"CRLF);
SAVE;
RETURN;
END "ADERR";
END "ERROR";
⊃ NOW WE HAVE A GOOD SET OF POT READINGS, UPDATE THE TRANSFORM;
PANTIL_CAM(CAMLENS,PANPOT,TILPOT,FOCPOT,ZOOPOT,MCOL,MICOL,LCEN);
⊃ FINISH UPDATE;
IF CAMNO=2 THEN
BEGIN "CM2FIX"
FOC[5]←C1+ZOOPOT*C2;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
FOCLENG[5]←FPOTD[5]/KY;
L4←P4;
L5←P5;
L6←P6;
L6←P7;
L8←P8;
END "CM2FIX";
REF ← IF CAMNUM THEN CREF ELSE SREF;
POTS[1,CAMNUM] ← PANPOT/REF;
POTS[2,CAMNUM] ← TILPOT/REF;
POTS[3,CAMNUM] ← FOCPOT/REF;
POTS[4,CAMNUM] ← IF CAMNUM THEN ZOOPOT/REF ELSE 0;
POTS[5,CAMNUM] ← IPOT/REF;
⊃ Now to update the global model;
ARRBLT (MODEL[1,1],MCOL[1,1],9);
ARRBLT (MODEL[6,1],MICOL[1,1],9);
ARRBLT (MODEL[4,1],LCEN[1],3);
MODEL[5,1] ← PP[CAMLENS,1];
MODEL[5,2] ← PP[CAMLENS,2];
MODEL[5,3] ← 1.0;
MODEL[9,1] ← CAMPAN ← (PPOTD[CAMLENS]*PANPOT+PPOT0[CAMLENS])*DEG;
MODEL[9,2] ← CAMTIL ← (TPOTD[CAMLENS]*TILPOT+TPOT0[CAMLENS])*DEG;
MODEL[9,3] ← CAMRANG ← LNS(FOCLEN0[CAMLENS]+FOCLENG[CAMLENS]*
FOCPOT,CAMLENS);
MODEL[10,3] ← IPOT;
MODEL[10,2] ← CAMZOOM ← IF CAMNUM=1 THEN CAMLENS ELSE FOC[5]*25.4;
MODEL[10,1]←CAMNO;
CURCAM[camno] ← GLOBAL NEW (MODEL);
IF DEB_CAM THEN OUTSTR(
"CAM_UPDATE: CAMPAN="&CVF(CAMPAN)&" DEGREES"CRLF&
" CAMTIL="&CVF(CAMTIL)&" DEGREES"CRLF&
" CAMRANG="&CVF(CAMRANG)&" INCHES"CRLF&
" CAMIRIS="&CVF(IPOT)&" UNITS"CRLF&
(IF CAMNO=2 THEN " CAMZOOM="&CVF(CAMZOOM)&" MM" ELSE NULL)
CRLF);
SAVE;
END "CAM_UPDATE";
⊃ ERROR CHECKING ROUTINES;
SIMPLE BOOLEAN PROCEDURE BADFOC(REAL LL1; STRING PROC);
IF ¬(FLIMIT2[CAMNUM]≤LL1≤FLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": FOCUS OUT OF BOUNDS, READING="&CVF(LL1)CRLF);
ERROR('100);
RETURN(TRUE);
END ELSE RETURN(FALSE);
SIMPLE PROCEDURE BADTIL(REAL LL2; STRING PROC);
IF ¬(TLIMIT2[CAMNUM]≤LL2≤TLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": TILT OUT OF BOUNDS, READING="&CVG(LL2)CRLF);
ERROR('40);
END;
SIMPLE PROCEDURE BADPAN(REAL LL3; STRING PROC);
IF ¬(PLIMIT2[CAMNUM]≤LL3≤PLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": PAN OUT OF BOUNDS, READING="&CVF(LL3)CRLF);
ERROR('20);
END;
SIMPLE PROCEDURE BADLEN(INTEGER RLENS; STRING PROC);
BEGIN
SERVO;
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
OUTSTR(PROC&": I GOOFED, NOW YOU HAVE"& " LENS NO. "&
CVS(LENS+1)CRLF);
ERROR('1000);
END;
END;
SIMPLE BOOLEAN PROCEDURE LENBND(INTEGER RLENS; STRING PROC);
IF ¬(1≤RLENS≤4) THEN
BEGIN
OUTSTR(PROC&": LENS NO. OUT OF BOUNDS -"&CVS(RLENS)CRLF);
ERROR('10);
RETURN(TRUE);
END ELSE RETURN(FALSE);
SIMPLE PROCEDURE BADIRIS(REAL LL3; STRING PROC);
IF ¬(ILIM1[CAMNUM]≤LL3≤ILIM2[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": IRIS OUT OF BOUNDS, READING="&CVF(LL3)CRLF);
ERROR('400);
END;
SIMPLE PROCEDURE BADZOOM(REAL LL7; STRING PROC);
IF ¬(ZLIM1≤LL7≤ZLIM2) THEN
BEGIN
OUTSTR(PROC&": ZOOM OUT OF BOUNDS, READING="&CVF(LL7)CRLF);
ERROR('200);
END;
⊃ CHANGE LENS OR MOVE PAN/TILT HEAD;
SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS);
BEGIN
CAMERA(1);
IF ¬LENBND(RLENS,"CHNG_LENS") THEN
BEGIN
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
LENS←RLENS-1;
STATUS←'20;
BADLEN(RLENS,"CHNG_LENS");
END;
CAMLENS←LENS+1;
CAM_UPDATE(1);
END;
SAVE;
END "CHNG_LENS";
SIMPLE MESSAGE PROCEDURE MOVE_CAM(INTEGER CAMNO;REAL PAN,TILT);
BEGIN REAL LL2,LL3;
CAMERA(CAMNO);
ref← if camno=1 then cref else GROREF[5];
PAN ← PAN/DEG;
TILT ← TILT/DEG;
LL3←(PAN-PPOT0[CAMLENS])/PPOTD[CAMLENS];
LL2←(TILT-TPOT0[CAMLENS])/TPOTD[CAMLENS];
IF CAMNO=1 THEN
BEGIN
L3←LL3/REF;
L2←LL2/REF;
L1←P1;
END ELSE BEGIN
L4←LL3/REF;
L5←LL2/REF;
L6←P6;
L7←P7;
L8←P8;
END;
BADTIL(LL2,"MOVE_CAM");
BADPAN(LL3,"MOVE_CAM");
IF CAMFLG=0 THEN
BEGIN
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END ELSE SAVE;
END "MOVE_CAM";
⊃ CHANGE LENS FOCUS AND ZOOM;
SIMPLE MESSAGE PROCEDURE CHNG_FOCUS(INTEGER CAMNO;REAL RANG);
BEGIN REAL LL1;
CAMERA(CAMNO);
ref← if camno=1 then cref else GROREF[5];
IF CAMNO=2 THEN
BEGIN
FOCLENG[5]←FPOTD[5]/KY;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
L4←P4;
L5←P5;
L7←P7;
L8←P8;
END;
LL1←(LNS(RANG,CAMLENS)-FOCLEN0[CAMLENS])/FOCLENG[CAMLENS];
IF BADFOC(LL1,"CHNG_FOCUS") THEN BEGIN SAVE;RETURN;END;
IF CAMNO=1 THEN L1←LL1/REF ELSE L6←LL1/REF;
L2←P2;
L3←P3;
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END "CHNG_FOCUS";
SIMPLE MESSAGE PROCEDURE CHNG_ZOOM(REAL FOC_LGTH);
BEGIN "CHNG_ZOOM" REAL LL7;
CAMERA(2);
L4←P4;
L5←P5;
L6←P6;
L8←P8;
LL7←(FOC_LGTH/25.4-C1)/C2;
L7 ← LL7/GROREF[5];
BADZOOM(LL7,"CHNG_ZOOM");
IF ¬CAMFLG THEN
BEGIN
SERVO;
CAM_UPDATE(2);
END;
END "CHNG_ZOOM";
⊃ CHANGE IRIS;
SIMPLE MESSAGE PROCEDURE CHNG_IRIS(INTEGER CAMNO; REAL IRIS);
BEGIN "CHNG_IRIS"
CAMERA(CAMNO);
REF ← IF CAMNO=1 THEN CREF ELSE GROREF[5];
IF CAMNO=1 THEN
BEGIN "CAM1IR"
OUTSTR("NO IRIS ON COHU YET" CRLF);
ERROR('400);
RETURN;
END "CAM1IR";
BADIRIS(IRIS,"CHNG_IRIS");
IF ¬CAMFLG THEN
BEGIN
L8 ← IRIS;
L7 ← P7;
L6 ← P6;
L5 ← P5;
L4 ← P4;
SERVO;
CAM_UPDATE(CAMNO);
END;
END "CHNG_IRIS";
⊃ CALCULATIONS COMMON TO PREDICTION AND CENTERING;
SIMPLE PROCEDURE CALC(INTEGER RLENS; REAL XTC,YTC,ZTC;REFERENCE REAL LL1,
LL2,LL3,LL4,ZOOM; STRING NAME);
BEGIN
REAL XCC,YCC,ZCC,D;
XCC ← P0[RLENS,1]-XTC;
YCC ← P0[RLENS,2]-YTC;
ZCC ← P0[RLENS,3]-ZTC;
ZOOM ← ZOOM/25.4;
PAN ← COSQR(YCC,-XCC,DP[RLENS,1]);
IF (CAMNUM=2) ∧ (P0[RLENS,2]+DP[RLENS,1]-YTC<0) THEN PAN ← -PAN;
D ← XCC*COS(PAN)+YCC*SIN(PAN);
TILT ← COSQR(ZCC,-D,-DP[RLENS,2]);
RANG ← D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
LL4 ← IF ZOOM THEN (ZOOM-C1)/C2 ELSE ZOOPOT;
LL3 ← (PAN-PPOT0[RLENS])/PPOTD[RLENS];
LL2 ← (TILT-TPOT0[RLENS])/TPOTD[RLENS];
LL1 ← (LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
BADTIL(LL2,NAME);
BADPAN(LL3,NAME);
BADFOC(LL1,NAME);
IF CAMNUM=2 THEN BADZOOM(LL4,NAME);
END;
⊃ CENTER CAMERA;
SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC,
ZOOM);
BEGIN REAL LL1,LL2,LL3,LL4;
CAMERA(CAMNO);
ref← if camno=1 then cref else sref;
IF CAMNO=1∧LENBND(RLENS,"CAM_CENTER") THEN BEGIN SAVE; RETURN; END
ELSE CAMLENS←RLENS;
CALC(CAMLENS, XTC, YTC, ZTC, LL1, LL2, LL3, LL4, ZOOM, "CAM_CENTER");
IF CAMFLG≠0 THEN BEGIN SAVE; RETURN; END;
IF CAMNO=1 THEN GROREF[CAMLENS]←CREF;
L1←L6←LL1/GROREF[CAMLENS];
L2←L5←LL2/GROREF[CAMLENS];
L3←L4←LL3/GROREF[CAMLENS];
IF CAMNO=2 THEN L7←LL4/GROREF[CAMLENS];
IF CAMNO=1 THEN
BEGIN
CALLEN;
IF LENS≠CAMLENS-1 THEN
BEGIN
LENS←CAMLENS-1;
STATUS←'30;
END ELSE STATUS←'10;
BADLEN(CAMLENS,"CAM_CENTER");
CAMLENS←LENS+1;
END ELSE SERVO;
CAM_UPDATE(camno);
END "CAM_CENTER";
⊃ TRANSFORM PREDICTION AND PROGRAM INITIALIZATION;
SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC,ZOOM);
BEGIN
REAL LL1,LL2,LL3,LL4;
CAMERA(CAMNO);
IF CAMNO=1∧LENBND(RLENS,"CAM_PRED") THEN BEGIN SAVE; RETURN; END ELSE
CAMLENS ← RLENS;
CALC(CAMLENS,XTC,YTC,ZTC,LL1,LL2,LL3,LL4,ZOOM,"CAM_PRED");
IF CAMFLG≠0 THEN BEGIN SAVE; RETURN; END;
PANTIL_CAM (RLENS,LL3,LL2,LL1,LL4,MCOL,MICOL,LCEN);
⊃ temporary hack;
ARRBLT (PREDIC[1,1],MCOL[1,1],9);
ARRBLT (PREDIC[6,1],MICOL[1,1],9);
ARRBLT (PREDIC[4,1],LCEN[1],3);
PREDIC[5,1] ← PP[CAMLENS,1];
PREDIC[5,2] ← PP[CAMLENS,2];
PREDIC[5,3] ← 1.0;
PREDIC[9,1] ← PAN/DEG;
PREDIC[9,2] ← TILT/DEG;
PREDIC[9,3] ← RANG;
PREDIC[10,1] ← camno;
PREDIC[10,2] ← IF CAMNO=1 THEN CAMLENS ELSE (LL4*C1+C2)/25.4;
PREDIC[10,3] ← CAMFLG;
SAVE;
END "CAM_PRED";
SIMPLE MESSAGE PROCEDURE CAM_INIT(INTEGER CAMNO);
BEGIN
CAMERA(CAMNO);
IF CAMNO=1 THEN
FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR;
IF CAMNO=2 THEN
BEGIN
BLOCK←1;
DATXFR;
END;
SAVE;
END"CAM_INIT";
⊃ PROGRAM TEST ROUTINE;
SIMPLE BOOLEAN PROCEDURE TESTIT;
BEGIN
STRING COORD;
REAL X,Y,Z,NPAN,NTILT,NRANG;
INTEGER NLENS,REQUEST;
STRING ST;
LABEL TES, TEE;
TES: OUTSTR("CAMERA NO.?"CRLF);
CAMNUM ← IF ST←INCHWL="1" THEN 1 ELSE 2;
TEE: OUTSTR("0-EXIT"CRLF&
"1-CHANGE THE LENS"CRLF&
"2-PAN AND TILT THE CAMERA"CRLF&
"3-FOCUS ON A GIVEN RANGE"CRLF&
"4-CENTER THE CAMERA ON A GIVEN POINT"CRLF&
"5-COMPLEMENT DEB_CAM"CRLF&
"6-CHANGE THE ZOOM"CRLF&
"7-UPDATE"CRLF&
"8-PRINT LIMITS"CRLF&
"9-CHANGE IRIS"CRLF);
OUTSTR("...TYPE THE NO. OF THE PROCEDURE YOU WANT TO TEST NOW="CRLF);
REQUEST←CVD(INCHWL);
IF ¬(0≤REQUEST≤9) THEN
BEGIN
OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&
CVS(REQUEST)&")"CRLF);
GOTO TES;
END;
CAMFLG ← 0;
CASE REQUEST OF
BEGIN
RETURN(FALSE); ⊃ 0;
BEGIN ⊃ 1;
OUTSTR("...TYPE NO. OF LENS="CRLF);
NLENS←CVD(INCHWL);
CHNG_LENS(NLENS);
END;
BEGIN ⊃ 2;
OUTSTR("...TYPE PAN AND TILT IN DEGREES,"&
" EACH FOLLOWED BY C.R="CRLF);
COORD←INCHWL;
NPAN←REALSCAN(COORD,MESS);
COORD←INCHWL;
NTILT←REALSCAN(COORD,MESS);
MOVE_CAM(CAMNUM,NPAN,NTILT);
END;
⊃ CONTINUE TESTING;
BEGIN ⊃ 3;
OUTSTR("...TYPE RANGE TO FOCUS ON="CRLF);
COORD←INCHWL;
NRANG←REALSCAN(COORD,MESS);
CHNG_FOCUS(CAMNUM,NRANG);
END;
BEGIN ⊃ 4;
OUTSTR("TYPE IN LENS N0. AND X,Y,Z OF CENTER IN TABLE"&
" COORDINATES AND FOCAL LENGTH"
CRLF&"EACH FOLLOWED BY A C.R.="CRLF);
COORD←INCHWL;
NLENS←CVD(COORD);
COORD←INCHWL;
X←REALSCAN(COORD,MESS);
COORD←INCHWL;
Y←REALSCAN(COORD,MESS);
COORD←INCHWL;
Z←REALSCAN(COORD,MESS);
COORD←INCHWL;
FOC_LGTH ← REALSCAN(COORD,MESS);
CAM_CENTER(NLENS,CAMNUM,X,Y,Z,FOC_LGTH);
END;
BEGIN ⊃ 5; DEB_CAM←¬DEB_CAM; GOTO TEE;END;
BEGIN ⊃ 6;
OUTSTR("DESIRED FOCAL LENGTH"CRLF);
COORD←INCHWL;
FOC_LGTH←REALSCAN(COORD,MESS);
CHNG_ZOOM(FOC_LGTH);
END;
BEGIN ⊃ 7;
INTEGER SAV;
SAV ← DEB_CAM; DEB_CAM ← TRUE;
CAM_UPDATE(CAMNUM); DEB_CAM ← SAV;
END;
LIMPRT;
BEGIN ⊃ 9;
OUTSTR("DESIRED IRIS POSITION"CRLF);
COORD ← INCHWL;
CHNG_IRIS(CAMNUM, REALSCAN(COORD,MESS));
END;
END;
RETURN(TRUE);
END "TESTIT";
⊃ THIS IS THE MAIN PROGRAM;
CAMFLG←0;
TYP_CAM←TRUE;
DEB_CAM←FALSE;
YES_CAM←TRUE;
JOB ← CVSIX("TTY");
FOR MESS ← 1,2 DO
BEGIN
CAMERR[MESS] ← 0;
CAMJOB[MESS] ← JOB;
END;
FOR CAMNUM←1 STEP 1 UNTIL 2 DO
BEGIN
CAM_INIT(CAMNUM);
IF CAMNUM=1 THEN
BEGIN
CALLEN;
CAMLENS←LENS+1;
END ELSE CAMLENS←5;
CAM_UPDATE(CAMNUM);
END;
IF RUN=0 THEN
BEGIN
OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
IF YES THEN WHILE TESTIT DO;
END ;
PUT_DATA(0,0,"CAM"); ⊃ DECLARE YOUR NAME ;
OUTSTR("CAM-ACTIVATED"CRLF);
WHILE TRUE DO
BEGIN
MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
CAMFLG←0;
JOB ← CVSIX(GET_DATA(1,MESS));
MESS←QUEUE('600,MESS); ⊃ ACTIVATE AND ACKNOWLEDGE ;
END;
END "GILCAM";